/*
 * Copyright (c) 2015 - 2016, Freescale Semiconductor, Inc.
 * Copyright 2016 - 2017 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
#include <stdio.h>
#include <stdlib.h>
/*${standard_header_anchor}*/
#include <stdbool.h>
#include "usb_device_config.h"
#include "usb.h"
#include "usb_device.h"

#include "usb_device_class.h"
#include "usb_device_hid.h"

#include "usb_device_ch9.h"
#include "usb_device_descriptor.h"

#include "composite.h"

#include "hid_keyboard.h"
#include "hid_mouse.h"

#include "fsl_device_registers.h"
#include "fsl_debug_console.h"
#include "pin_mux.h"
#include "clock_config.h"
#include "board.h"
#include "peripherals.h"
#include "./lsm6ds3tr-c-pid/lsm6ds3tr-c_reg.h"
#include "./lis2mdl-pid/lis2mdl_reg.h"

#include "lwrb.h"

#if (defined(FSL_FEATURE_SOC_SYSMPU_COUNT) && (FSL_FEATURE_SOC_SYSMPU_COUNT > 0U))
#include "fsl_sysmpu.h"
#endif /* FSL_FEATURE_SOC_SYSMPU_COUNT */

#if (USB_DEVICE_CONFIG_HID < 2U)
#error USB_DEVICE_CONFIG_HID need to > 1U, Please change the MARCO USB_DEVICE_CONFIG_HID in file "usb_device_config.h".
#endif

#include "usb_phy.h"
/*******************************************************************************
 * Definitions
 ******************************************************************************/

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
void BOARD_InitHardware(void);
void USB_DeviceClockInit(void);
void USB_DeviceIsrEnable(void);
#if USB_DEVICE_CONFIG_USE_TASK
void USB_DeviceTaskFn(void *deviceHandle);
#endif

static usb_status_t USB_DeviceCallback(usb_device_handle handle, uint32_t event, void *param);
static void USB_DeviceApplicationInit(void);

/*******************************************************************************
 * Variables
 ******************************************************************************/

extern usb_device_class_struct_t g_UsbDeviceHidMouseConfig;
extern usb_device_class_struct_t g_UsbDeviceHidKeyboardConfig;

usb_device_composite_struct_t g_UsbDeviceComposite;

/* Set class configurations */
usb_device_class_config_struct_t g_CompositeClassConfig[USB_COMPOSITE_INTERFACE_COUNT] = {
    {
        USB_DeviceHidKeyboardCallback, /* HID keyboard class callback pointer */
        (class_handle_t)NULL,          /* The HID class handle, This field is set by USB_DeviceClassInit */
        &g_UsbDeviceHidKeyboardConfig, /* The HID keyboard configuration, including class code, subcode, and protocol,
                                  class
                                  type, transfer type, endpoint address, max packet size, etc.*/
    },
    {
        USB_DeviceHidMouseCallback, /* HID mouse class callback pointer */
        (class_handle_t)NULL,       /* The HID class handle, This field is set by USB_DeviceClassInit */
        &g_UsbDeviceHidMouseConfig, /* The HID mouse configuration, including class code, subcode, and protocol, class
                               type,
                               transfer type, endpoint address, max packet size, etc.*/
    }};

/* Set class configuration list */
usb_device_class_config_list_struct_t g_UsbDeviceCompositeConfigList = {
    g_CompositeClassConfig,        /* Class configurations */
    USB_DeviceCallback,            /* Device callback pointer */
    USB_COMPOSITE_INTERFACE_COUNT, /* Class count */
};

/*******************************************************************************
 * Code
 ******************************************************************************/

void USB_OTG1_IRQHandler(void)
{
    USB_DeviceEhciIsrFunction(g_UsbDeviceComposite.deviceHandle);
}

void USB_DeviceClockInit(void)
{
    usb_phy_config_struct_t phyConfig = {
        BOARD_USB_PHY_D_CAL,
        BOARD_USB_PHY_TXCAL45DP,
        BOARD_USB_PHY_TXCAL45DM,
    };

    CLOCK_EnableUsbhs0PhyPllClock(kCLOCK_Usbphy480M, 480000000U);
    CLOCK_EnableUsbhs0Clock(kCLOCK_Usb480M, 480000000U);
    USB_EhciPhyInit(CONTROLLER_ID, BOARD_XTAL0_CLK_HZ, &phyConfig);
}
void USB_DeviceIsrEnable(void)
{
    uint8_t irqNumber;

    uint8_t usbDeviceEhciIrq[] = USBHS_IRQS;
    irqNumber                  = usbDeviceEhciIrq[CONTROLLER_ID - kUSB_ControllerEhci0];

    /* Install isr, set priority, and enable IRQ. */
    NVIC_SetPriority((IRQn_Type)irqNumber, USB_DEVICE_INTERRUPT_PRIORITY);
    EnableIRQ((IRQn_Type)irqNumber);
}
#if USB_DEVICE_CONFIG_USE_TASK
void USB_DeviceTaskFn(void *deviceHandle)
{
    USB_DeviceEhciTaskFunction(deviceHandle);
}
#endif

/* The Device callback */
static usb_status_t USB_DeviceCallback(usb_device_handle handle, uint32_t event, void *param)
{
    usb_status_t error = kStatus_USB_InvalidRequest;
    uint16_t *temp16   = (uint16_t *)param;
    uint8_t *temp8     = (uint8_t *)param;

    switch (event)
    {
        case kUSB_DeviceEventBusReset:
        {
            /* USB bus reset signal detected */
            g_UsbDeviceComposite.attach               = 0U;
            g_UsbDeviceComposite.currentConfiguration = 0U;
            error                                     = kStatus_USB_Success;
#if (defined(USB_DEVICE_CONFIG_EHCI) && (USB_DEVICE_CONFIG_EHCI > 0U)) || \
    (defined(USB_DEVICE_CONFIG_LPCIP3511HS) && (USB_DEVICE_CONFIG_LPCIP3511HS > 0U))
            /* Get USB speed to configure the device, including max packet size and interval of the endpoints. */
            if (kStatus_USB_Success == USB_DeviceClassGetSpeed(CONTROLLER_ID, &g_UsbDeviceComposite.speed))
            {
                USB_DeviceSetSpeed(handle, g_UsbDeviceComposite.speed);
            }
#endif
        }
        break;
        case kUSB_DeviceEventSetConfiguration:
            if (0U == (*temp8))
            {
                g_UsbDeviceComposite.attach               = 0U;
                g_UsbDeviceComposite.currentConfiguration = 0U;
                error                                     = kStatus_USB_Success;
            }
            else if (USB_COMPOSITE_CONFIGURE_INDEX == (*temp8))
            {
                /* Set device configuration request */
                g_UsbDeviceComposite.attach = 1U;
                USB_DeviceHidMouseSetConfigure(g_UsbDeviceComposite.hidMouseHandle, *temp8);
                USB_DeviceHidKeyboardSetConfigure(g_UsbDeviceComposite.hidKeyboardHandle, *temp8);
                g_UsbDeviceComposite.currentConfiguration = *temp8;
                error                                     = kStatus_USB_Success;
            }
            else
            {
                /* no action, return kStatus_USB_InvalidRequest. */
            }
            break;
        case kUSB_DeviceEventSetInterface:
            if (g_UsbDeviceComposite.attach)
            {
                /* Set device interface request */
                uint8_t interface        = (uint8_t)((*temp16 & 0xFF00U) >> 0x08U);
                uint8_t alternateSetting = (uint8_t)(*temp16 & 0x00FFU);
                if (USB_HID_MOUSE_INTERFACE_INDEX == interface)
                {
                    if (alternateSetting < USB_HID_MOUSE_INTERFACE_ALTERNATE_COUNT)
                    {
                        g_UsbDeviceComposite.currentInterfaceAlternateSetting[interface] = alternateSetting;
                        USB_DeviceHidMouseSetInterface(g_UsbDeviceComposite.hidMouseHandle, interface, alternateSetting);
                        error = kStatus_USB_Success;
                    }
                }
                else if (USB_HID_KEYBOARD_INTERFACE_INDEX == interface)
                {
                    if (alternateSetting < USB_HID_KEYBOARD_INTERFACE_ALTERNATE_COUNT)
                    {
                        g_UsbDeviceComposite.currentInterfaceAlternateSetting[interface] = alternateSetting;
                        USB_DeviceHidKeyboardSetInterface(g_UsbDeviceComposite.hidKeyboardHandle, interface,
                                                          alternateSetting);
                        error = kStatus_USB_Success;
                    }
                }
                else
                {
                    /* no action, return kStatus_USB_InvalidRequest. */
                }
            }
            break;
        case kUSB_DeviceEventGetConfiguration:
            if (param)
            {
                /* Get current configuration request */
                *temp8 = g_UsbDeviceComposite.currentConfiguration;
                error  = kStatus_USB_Success;
            }
            break;
        case kUSB_DeviceEventGetInterface:
            if (param)
            {
                /* Get current alternate setting of the interface request */
                uint8_t interface = (uint8_t)((*temp16 & 0xFF00U) >> 0x08U);
                if (interface < USB_COMPOSITE_INTERFACE_COUNT)
                {
                    *temp16 = (*temp16 & 0xFF00U) | g_UsbDeviceComposite.currentInterfaceAlternateSetting[interface];
                    error   = kStatus_USB_Success;
                }
            }
            break;
        case kUSB_DeviceEventGetDeviceDescriptor:
            if (param)
            {
                /* Get device descriptor request */
                error = USB_DeviceGetDeviceDescriptor(handle, (usb_device_get_device_descriptor_struct_t *)param);
            }
            break;
        case kUSB_DeviceEventGetConfigurationDescriptor:
            if (param)
            {
                /* Get device configuration descriptor request */
                error = USB_DeviceGetConfigurationDescriptor(handle,
                                                             (usb_device_get_configuration_descriptor_struct_t *)param);
            }
            break;
        case kUSB_DeviceEventGetStringDescriptor:
            if (param)
            {
                /* Get device string descriptor request */
                error = USB_DeviceGetStringDescriptor(handle, (usb_device_get_string_descriptor_struct_t *)param);
            }
            break;
        case kUSB_DeviceEventGetHidDescriptor:
            if (param)
            {
                /* Get hid descriptor request */
                error = USB_DeviceGetHidDescriptor(handle, (usb_device_get_hid_descriptor_struct_t *)param);
            }
            break;
        case kUSB_DeviceEventGetHidReportDescriptor:
            if (param)
            {
                /* Get hid report descriptor request */
                error =
                    USB_DeviceGetHidReportDescriptor(handle, (usb_device_get_hid_report_descriptor_struct_t *)param);
            }
            break;
#if (defined(USB_DEVICE_CONFIG_CV_TEST) && (USB_DEVICE_CONFIG_CV_TEST > 0U))
        case kUSB_DeviceEventGetDeviceQualifierDescriptor:
            if (param)
            {
                /* Get Qualifier descriptor request */
                error = USB_DeviceGetDeviceQualifierDescriptor(
                    handle, (usb_device_get_device_qualifier_descriptor_struct_t *)param);
            }
            break;
#endif
        case kUSB_DeviceEventGetHidPhysicalDescriptor:
            if (param)
            {
                /* Get hid physical descriptor request */
                error = USB_DeviceGetHidPhysicalDescriptor(handle,
                                                           (usb_device_get_hid_physical_descriptor_struct_t *)param);
            }
            break;
        default:
            break;
    }

    return error;
}

/* Application initialization */
static void USB_DeviceApplicationInit(void)
{
    USB_DeviceClockInit();
#if (defined(FSL_FEATURE_SOC_SYSMPU_COUNT) && (FSL_FEATURE_SOC_SYSMPU_COUNT > 0U))
    SYSMPU_Enable(SYSMPU, 0);
#endif /* FSL_FEATURE_SOC_SYSMPU_COUNT */

    /* Set composite device to default state */
    g_UsbDeviceComposite.speed             = USB_SPEED_FULL;
    g_UsbDeviceComposite.attach            = 0U;
    g_UsbDeviceComposite.hidMouseHandle    = (class_handle_t)NULL;
    g_UsbDeviceComposite.hidKeyboardHandle = (class_handle_t)NULL;
    g_UsbDeviceComposite.deviceHandle      = NULL;

    if (kStatus_USB_Success !=
        USB_DeviceClassInit(CONTROLLER_ID, &g_UsbDeviceCompositeConfigList, &g_UsbDeviceComposite.deviceHandle))
    {
        usb_echo("USB device composite demo init failed\r\n");
        return;
    }
    else
    {
        usb_echo("USB device composite demo\r\n");
        /* Get the HID keyboard class handle */
        g_UsbDeviceComposite.hidKeyboardHandle = g_UsbDeviceCompositeConfigList.config[0].classHandle;
        /* Get the HID mouse class handle */
        g_UsbDeviceComposite.hidMouseHandle = g_UsbDeviceCompositeConfigList.config[1].classHandle;

        USB_DeviceHidKeyboardInit(&g_UsbDeviceComposite);
        USB_DeviceHidMouseInit(&g_UsbDeviceComposite);
    }

    USB_DeviceIsrEnable();

    /* Start the device function */
    /*Add one delay here to make the DP pull down long enough to allow host to detect the previous disconnection.*/
    SDK_DelayAtLeastUs(5000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY);
    USB_DeviceRun(g_UsbDeviceComposite.deviceHandle);
}

#if USB_DEVICE_CONFIG_USE_TASK
void USB_DeviceTask(void *handle)
{
    while (1U)
    {
        USB_DeviceTaskFn(handle);
    }
}
#endif

#define ENCODE_PORT GPIO2
#define ENCODE_D_PIN 5U
#define ENCODE_A_PIN 7U
#define ENCODE_B_PIN 9U

typedef enum _msg_type {
  ROTARY_RIGHT   = 0x00,
  ROTARY_LEFT    = 0x01,
  JOYSTICK_EVENT = 0x02,
  MOUSE_EVENT    = 0x03,
} msg_type;

typedef struct _msg_buffer {
  msg_type type;
  int8_t data[4];
} msg_buffer;

#define MSG_BUFFER_SIZE 50
msg_buffer msg_buff[MSG_BUFFER_SIZE];
lwrb_t msg;

void GPIO2_GPIO_COMB_0_15_IRQHANDLER(void) {
  BaseType_t pxHigherPriorityTaskWoken = pdFALSE;
  msg_buffer send_data;
  static uint8_t firstKeyDetect;

  if (GPIO_GetPinsInterruptFlags(ENCODE_PORT) & (1U << ENCODE_A_PIN)) {
    /* clear the interrupt status */
    GPIO_PortClearInterruptFlags(ENCODE_PORT, 1U << ENCODE_A_PIN);
    /* Change state of switch. */

    if (GPIO_PinRead(ENCODE_PORT, ENCODE_A_PIN)) {  // Rising edge
      if (GPIO_PinRead(ENCODE_PORT, ENCODE_B_PIN) && (firstKeyDetect == ROTARY_LEFT)) {
        send_data.type = ROTARY_LEFT;
        lwrb_write(&msg, &send_data, sizeof(msg_buffer));
      } else if (!GPIO_PinRead(ENCODE_PORT, ENCODE_B_PIN) && (firstKeyDetect == ROTARY_RIGHT)) {
        send_data.type = ROTARY_RIGHT;
        lwrb_write(&msg, &send_data, sizeof(msg_buffer));
      }
    } else {  // Falling edge
      if (GPIO_PinRead(ENCODE_PORT, ENCODE_B_PIN)) {
        firstKeyDetect = ROTARY_RIGHT;
      } else {
        firstKeyDetect = ROTARY_LEFT;
      }
    }

  } else if (GPIO_GetPinsInterruptFlags(ENCODE_PORT) & (1U << ENCODE_D_PIN)) {
    /* clear the interrupt status */
    GPIO_PortClearInterruptFlags(ENCODE_PORT, 1U << ENCODE_D_PIN);
    /* Change state of switch. */

    send_data.type    = MOUSE_EVENT;
    send_data.data[0] = (GPIO_PinRead(ENCODE_PORT, ENCODE_D_PIN) == 0) ? 1 : 0;
    lwrb_write(&msg, &send_data, sizeof(msg_buffer));
  }

  SDK_ISR_EXIT_BARRIER;
}

void APP_task(void *handle) {
  msg_buffer ret_data;
  uint8_t isMousePressed = pdFALSE;

  USB_DeviceApplicationInit();
#if USB_DEVICE_CONFIG_USE_TASK
  if (g_UsbDeviceComposite.deviceHandle) {
    if (xTaskCreate(USB_DeviceTask,                        /* pointer to the task */
                    "usb device task",                     /* task name for kernel awareness debugging */
                    5000L / sizeof(portSTACK_TYPE),        /* task stack size */
                    g_UsbDeviceComposite.deviceHandle,     /* optional task startup argument */
                    5U,                                    /* initial priority */
                    &g_UsbDeviceComposite.deviceTaskHandle /* optional task handle to create */
                    ) != pdPASS) {
      usb_echo("usb device task create failed!\r\n");
      return;
    }
  }
#endif

  while (1U) {
    if (lwrb_get_linear_block_read_length(&msg) > 0) {
      lwrb_read(&msg, &ret_data, sizeof(msg_buffer));
      // usb_echo("ret_data %x\r\n", ret_data.type);
      switch (ret_data.type) {
        case ROTARY_LEFT:
          GPIO_PinWrite(BOARD_INITPINS_LED1_GPIO, BOARD_INITPINS_LED1_PIN, 0U);
          send_key_scale_up();
          vTaskDelay(10);
          send_key_null();
          GPIO_PinWrite(BOARD_INITPINS_LED1_GPIO, BOARD_INITPINS_LED1_PIN, 1U);
          break;

        case ROTARY_RIGHT:
          GPIO_PinWrite(BOARD_INITPINS_LED1_GPIO, BOARD_INITPINS_LED1_PIN, 0U);
          send_key_scale_down();
          vTaskDelay(10);
          send_key_null();
          GPIO_PinWrite(BOARD_INITPINS_LED1_GPIO, BOARD_INITPINS_LED1_PIN, 1U);
          break;

        case JOYSTICK_EVENT:
          // usb_echo("px: %d py: %d\r\n", ret_data.data[0], ret_data.data[1]);
          send_mouse_event(ret_data.data[0], ret_data.data[1], isMousePressed);
          break;

        case MOUSE_EVENT:
          // usb_echo("Mouse Key: %d\r\n", ret_data.data[0]);
          isMousePressed = ret_data.data[0];
          send_mouse_event(0, 0, isMousePressed);
          break;

        default:
          break;
      }
    } else {
      vTaskDelay(20);
    }
  }
}

// void Delay_us(uint32_t us) {
//   uint32_t delay = us * 1000;
//   do {
//     __NOP();
//   } while (delay--);
// }

#define JOYSTICK_PORT GPIO2
#define JOYSTICK_PIN 28U

#define DUTY_CYCLE_FULL_TIME 1000000

TaskHandle_t joystickTaskHandle;
void Joystick_Task(void *handle) {
  int32_t StartCount, HighLevelCount = 0, LowLevelCount = 0;
  msg_buffer send_data;
  int8_t px, py;

  while (1) {
    HighLevelCount = 0;
    LowLevelCount  = 0;
    StartCount     = 0;

    vTaskDelay(10);
    GPIO_PinWrite(BOARD_INITPINS_LED2_GPIO, BOARD_INITPINS_LED2_PIN, 1U);

    // usb_echo("tick_start %d\r\n", xTaskGetTickCount());
    if (GPIO_PinRead(JOYSTICK_PORT, JOYSTICK_PIN)) {
      //  Get start signal
      while (GPIO_PinRead(JOYSTICK_PORT, JOYSTICK_PIN)) {
        if (StartCount++ > DUTY_CYCLE_FULL_TIME) {
          break;
        }
      }
      if (StartCount > DUTY_CYCLE_FULL_TIME) {
        continue;
      }

      while (!GPIO_PinRead(JOYSTICK_PORT, JOYSTICK_PIN)) {
        LowLevelCount++;
        // Delay_us(1);
      }

      while (GPIO_PinRead(JOYSTICK_PORT, JOYSTICK_PIN)) {
        HighLevelCount++;
        // Delay_us(1);
      }

    } else {
      //  Get start signal
      while (!GPIO_PinRead(JOYSTICK_PORT, JOYSTICK_PIN)) {
        if (StartCount++ > DUTY_CYCLE_FULL_TIME) {
          break;
        }
      }
      if (StartCount > DUTY_CYCLE_FULL_TIME) {
        continue;
      }

      while (GPIO_PinRead(JOYSTICK_PORT, JOYSTICK_PIN)) {
        HighLevelCount++;
        // Delay_us(1);
      }

      while (!GPIO_PinRead(JOYSTICK_PORT, JOYSTICK_PIN)) {
        LowLevelCount++;
        // Delay_us(1);
      }
    }
    // usb_echo("tick_end %d\r\n", xTaskGetTickCount());

    // usb_echo("HighLevelCount: %d LowLevelCount: %d StartCount: %d\r\n", HighLevelCount, LowLevelCount, StartCount);
    px = ((HighLevelCount * 100) / (HighLevelCount + LowLevelCount) - 49) / 6;  // set to -5~5
    py = ((((HighLevelCount + LowLevelCount) * 100) / 40000) - 60) / 3;         // set to -5~5;

    // StartCount = usb_echo("StartCount: %d\r\n", StartCount);
    if ((px != 0) || (py != 0)) {
      //  usb_echo("px: %d py: %d\r\n", px, py);
      send_data.type    = JOYSTICK_EVENT;
      send_data.data[0] = px;
      send_data.data[1] = py;
      lwrb_write(&msg, &send_data, sizeof(msg_buffer));
      GPIO_PinWrite(BOARD_INITPINS_LED2_GPIO, BOARD_INITPINS_LED2_PIN, 0U);
    }
  }
  return;
}

#define XPT2046_CMD_S 0x80

#define XPT2046_CMD_MODE_12_BIT 0x80
#define XPT2046_CMD_MODE_8_BIT 0x00

#define XPT2046_CMD_TEST_Y 0x10
#define XPT2046_CMD_TEST_X 0x50

#define XPT2046_CMD_SER 0x04
#define XPT2046_CMD_DFR 0x00

#define XPT2046_CMD_ADC_EN 0x02
#define XPT2046_CMD_ADC_DISABLE 0x00

#define XPT2046_PENIRQ_EN 0x01
#define XPT2046_PENIRQ_DISABLE 0x00

#define XPT2046_CS_PORT BOARD_INITPINS_TOUCH_CS_GPIO
#define XPT2046_CS_PIN BOARD_INITPINS_TOUCH_CS_GPIO_PIN

int16_t getTouchPos(int8_t direction) {
  lpspi_transfer_t masterXfer;
  uint8_t masterRxData[2] = {0U};
  uint8_t masterTxData[1] = {0U};
  int16_t touchValue;

  GPIO_PinWrite(XPT2046_CS_PORT, XPT2046_CS_PIN, 0U);

  if (direction) {
    masterTxData[0] = XPT2046_CMD_S | XPT2046_CMD_MODE_12_BIT | XPT2046_CMD_TEST_Y | XPT2046_CMD_DFR |
                      XPT2046_CMD_ADC_EN | XPT2046_PENIRQ_DISABLE;
  } else {
    masterTxData[0] = XPT2046_CMD_S | XPT2046_CMD_MODE_12_BIT | XPT2046_CMD_TEST_X | XPT2046_CMD_DFR |
                      XPT2046_CMD_ADC_EN | XPT2046_PENIRQ_DISABLE;
  }
  masterXfer.txData      = masterTxData;
  masterXfer.rxData      = NULL;
  masterXfer.dataSize    = 1;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking(LPSPI4_PERIPHERAL, &masterXfer);

  vTaskDelay(10);
  masterXfer.txData      = NULL;
  masterXfer.rxData      = masterRxData;
  masterXfer.dataSize    = 2;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking(LPSPI4_PERIPHERAL, &masterXfer);
  vTaskDelay(1);

  GPIO_PinWrite(XPT2046_CS_PORT, XPT2046_CS_PIN, 1U);

  touchValue = (int16_t)masterRxData[0] * 256 + masterRxData[1];
  return touchValue;
}

int8_t getTouchStatus(void) {
  int8_t isTouchPressed = pdFALSE;
  int16_t touchX, touchY;

  touchX = getTouchPos(0);
  touchY = getTouchPos(1);

  // usb_echo("read data %x %x\r\n", touchX, touchY);

  if ((touchX > 0x0100 && touchX < 0x7C00) || (touchY > 0x0100 && touchY < 0x7C00)) {
    isTouchPressed = pdTRUE;
  }
  return isTouchPressed;
}

TaskHandle_t touchTaskHandle;
void Touch_Task(void *handle) {
  int8_t isTouchPressed = pdFALSE;
  msg_buffer send_data;

  while (1) {
    if (getTouchStatus() != isTouchPressed) {
      isTouchPressed = !isTouchPressed;

      send_data.type    = MOUSE_EVENT;
      send_data.data[0] = isTouchPressed;
      lwrb_write(&msg, &send_data, sizeof(msg_buffer));

      GPIO_PinWrite(BOARD_INITPINS_LED3_GPIO, BOARD_INITPINS_LED3_PIN, 0U);
    }
    vTaskDelay(100);
    GPIO_PinWrite(BOARD_INITPINS_LED3_GPIO, BOARD_INITPINS_LED3_PIN, 1U);
  }
  return;
}

#define LSM6DS3TR_C_CS_PORT BOARD_INITPINS_LSM6DS3TR_C_CS_PORT
#define LSM6DS3TR_C_CS_PIN BOARD_INITPINS_LSM6DS3TR_C_CS_PIN

/*
 * @brief  platform specific delay (platform dependent)
 *
 * @param  ms        delay in ms
 *
 */
static void platform_delay(uint32_t ms) { vTaskDelay(ms); }

/*
 * @brief  Write generic device register (platform dependent)
 *
 * @param  handle    customizable argument. In this examples is used in
 *                   order to select the correct sensor bus handler.
 * @param  reg       register to write
 * @param  bufp      pointer to data to write in register reg
 * @param  len       number of consecutive register to write
 *
 */
static int32_t lsm6ds3tr_platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len) {
  lpspi_transfer_t masterXfer;

  GPIO_PinWrite(LSM6DS3TR_C_CS_PORT, LSM6DS3TR_C_CS_PIN, 0U);

  masterXfer.txData      = &reg;
  masterXfer.rxData      = NULL;
  masterXfer.dataSize    = 1;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking((LPSPI_Type *)handle, &masterXfer);
  platform_delay(1);

  masterXfer.txData      = (uint8_t *)bufp;
  masterXfer.rxData      = NULL;
  masterXfer.dataSize    = len;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking((LPSPI_Type *)handle, &masterXfer);
  platform_delay(len % 50 + 1);

  GPIO_PinWrite(LSM6DS3TR_C_CS_PORT, LSM6DS3TR_C_CS_PIN, 1U);

  return 0;
}

/*
 * @brief  Read generic device register (platform dependent)
 *
 * @param  handle    customizable argument. In this examples is used in
 *                   order to select the correct sensor bus handler.
 * @param  reg       register to read
 * @param  bufp      pointer to buffer that store the data read
 * @param  len       number of consecutive register to read
 *
 */
static int32_t lsm6ds3tr_platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len) {
  lpspi_transfer_t masterXfer;

  GPIO_PinWrite(LSM6DS3TR_C_CS_PORT, LSM6DS3TR_C_CS_PIN, 0U);

  reg |= 0x80;
  masterXfer.txData      = &reg;
  masterXfer.rxData      = NULL;
  masterXfer.dataSize    = 1;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking((LPSPI_Type *)handle, &masterXfer);
  platform_delay(1);

  masterXfer.txData      = NULL;
  masterXfer.rxData      = bufp;
  masterXfer.dataSize    = len;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking((LPSPI_Type *)handle, &masterXfer);
  platform_delay(len % 50 + 1);

  GPIO_PinWrite(LSM6DS3TR_C_CS_PORT, LSM6DS3TR_C_CS_PIN, 1U);

  return 0;
}

stmdev_ctx_t lsm6ds3tr_c_ctx, lis2mdl_ctx;

void lsm6ds3tr_c_init(void) {
  uint8_t chipId = 0;
  uint8_t rst;

  lsm6ds3tr_c_ctx.write_reg = lsm6ds3tr_platform_write;
  lsm6ds3tr_c_ctx.read_reg  = lsm6ds3tr_platform_read;
  lsm6ds3tr_c_ctx.mdelay    = platform_delay;
  lsm6ds3tr_c_ctx.handle    = LPSPI2_PERIPHERAL;

  lsm6ds3tr_c_device_id_get(&lsm6ds3tr_c_ctx, &chipId);

  //  if (chipId != LSM6DS3TR_C_ID)
  //    while (1)
  //      ; /*manage here device not found */
  usb_echo("lsm6ds3tr_c chipId %x\n\r", chipId);

#if 1
  /* Restore default configuration */
  lsm6ds3tr_c_reset_set(&lsm6ds3tr_c_ctx, PROPERTY_ENABLE);

  do {
    lsm6ds3tr_c_reset_get(&lsm6ds3tr_c_ctx, &rst);
  } while (rst);

  /* Enable Block Data Update */
  lsm6ds3tr_c_block_data_update_set(&lsm6ds3tr_c_ctx, PROPERTY_ENABLE);
  /* Set Output Data Rate */
  lsm6ds3tr_c_xl_data_rate_set(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_XL_ODR_12Hz5);
  lsm6ds3tr_c_gy_data_rate_set(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_GY_ODR_12Hz5);
  /* Set full scale */
  lsm6ds3tr_c_xl_full_scale_set(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_2g);
  lsm6ds3tr_c_gy_full_scale_set(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_2000dps);
  /* Configure filtering chain(No aux interface) */
  /* Accelerometer - analog filter */
  lsm6ds3tr_c_xl_filter_analog_set(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_XL_ANA_BW_400Hz);
  /* Accelerometer - LPF1 path ( LPF2 not used )*/
  // lsm6ds3tr_c_xl_lp1_bandwidth_set(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_XL_LP1_ODR_DIV_4);
  /* Accelerometer - LPF1 + LPF2 path */
  lsm6ds3tr_c_xl_lp2_bandwidth_set(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100);
  /* Accelerometer - High Pass / Slope path */
  // lsm6ds3tr_c_xl_reference_mode_set(&lsm6ds3tr_c_ctx, PROPERTY_DISABLE);
  // lsm6ds3tr_c_xl_hp_bandwidth_set(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_XL_HP_ODR_DIV_100);
  /* Gyroscope - filtering chain */
  lsm6ds3tr_c_gy_band_pass_set(&lsm6ds3tr_c_ctx, LSM6DS3TR_C_HP_260mHz_LP1_STRONG);

#endif
}

#define LIS2MDL_CS_PORT BOARD_INITPINS_LIS2MDL_CS_PORT
#define LIS2MDL_CS_PIN BOARD_INITPINS_LIS2MDL_CS_PIN

int32_t lis2mdl_platform_write(void *handle, uint8_t Reg, const uint8_t *Bufp, uint16_t len) {
  lpspi_transfer_t masterXfer;

  GPIO_PinWrite(LIS2MDL_CS_PORT, LIS2MDL_CS_PIN, 0U);

  masterXfer.txData      = &Reg;
  masterXfer.rxData      = NULL;
  masterXfer.dataSize    = 1;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking((LPSPI_Type *)handle, &masterXfer);
  platform_delay(1);

  masterXfer.txData      = (uint8_t *)Bufp;
  masterXfer.rxData      = NULL;
  masterXfer.dataSize    = len;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking((LPSPI_Type *)handle, &masterXfer);
  platform_delay(len % 50 + 1);

  GPIO_PinWrite(LIS2MDL_CS_PORT, LIS2MDL_CS_PIN, 1U);

  return 0;
}

int32_t lis2mdl_platform_read(void *handle, uint8_t Reg, uint8_t *Bufp, uint16_t len) {
  lpspi_transfer_t masterXfer;

  GPIO_PinWrite(LIS2MDL_CS_PORT, LIS2MDL_CS_PIN, 0U);

  Reg |= 0x80;
  masterXfer.txData      = &Reg;
  masterXfer.rxData      = NULL;
  masterXfer.dataSize    = 1;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking((LPSPI_Type *)handle, &masterXfer);
  platform_delay(1);

  masterXfer.txData      = NULL;
  masterXfer.rxData      = Bufp;
  masterXfer.dataSize    = len;
  masterXfer.configFlags = kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
  LPSPI_MasterTransferBlocking((LPSPI_Type *)handle, &masterXfer);
  platform_delay(len % 50 + 1);

  GPIO_PinWrite(LIS2MDL_CS_PORT, LIS2MDL_CS_PIN, 1U);

  return 0;
}

void lis2mdl_init(void) {
  uint8_t chipId = 0;
  uint8_t rst;

  lis2mdl_ctx.write_reg = lis2mdl_platform_write;
  lis2mdl_ctx.read_reg  = lis2mdl_platform_read;
  lis2mdl_ctx.mdelay    = platform_delay;
  lis2mdl_ctx.handle    = LPSPI2_PERIPHERAL;

  lis2mdl_spi_mode_set(&lis2mdl_ctx, LIS2MDL_SPI_4_WIRE);

  lis2mdl_device_id_get(&lis2mdl_ctx, &chipId);
  usb_echo("lis2mdl chipId %x\n\r", chipId);

  /* Restore default configuration */
  lis2mdl_reset_set(&lis2mdl_ctx, PROPERTY_ENABLE);

  do {
    lis2mdl_reset_get(&lis2mdl_ctx, &rst);
  } while (rst);

  /* Enable Block Data Update */
  lis2mdl_block_data_update_set(&lis2mdl_ctx, PROPERTY_ENABLE);
  /* Set Output Data Rate */
  lis2mdl_data_rate_set(&lis2mdl_ctx, LIS2MDL_ODR_10Hz);
  /* Set / Reset sensor mode */
  lis2mdl_set_rst_mode_set(&lis2mdl_ctx, LIS2MDL_SENS_OFF_CANC_EVERY_ODR);
  /* Enable temperature compensation */
  lis2mdl_offset_temp_comp_set(&lis2mdl_ctx, PROPERTY_ENABLE);
  /* Set Low-pass bandwidth to ODR/4 */
  // lis2mdl_low_pass_bandwidth_set(&dev_ctx, LIS2MDL_ODR_DIV_4);
  /* Set device in continuous mode */
  lis2mdl_operating_mode_set(&lis2mdl_ctx, LIS2MDL_CONTINUOUS_MODE);
}

void sensor_init(void) {
  lsm6ds3tr_c_init();
  lis2mdl_init();
}

TaskHandle_t sensorTaskHandle;
void Sensor_Task(void *handle) {
  int16_t data_raw_acceleration[3];
  int16_t data_raw_angular_rate[3];
  int16_t data_raw_temperature;
  float acceleration_mg[3];
  float angular_rate_mdps[3];
  float temperature_degC;

  int16_t data_raw_magnetic[3];
  // int16_t data_raw_temperature;
  float magnetic_mG[3];
  // float temperature_degC;

  sensor_init();

  /* Read samples in polling mode (no int) */
  while (1) {
#if 0
	  lsm6ds3tr_c_reg_t reg;
    /* Read output only if new value is available */
    lsm6ds3tr_c_status_reg_get(&lsm6ds3tr_c_ctx, &reg.status_reg);

    if (reg.status_reg.xlda) {
      /* Read magnetic field data */
      memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
      lsm6ds3tr_c_acceleration_raw_get(&lsm6ds3tr_c_ctx, data_raw_acceleration);
      acceleration_mg[0] = lsm6ds3tr_c_from_fs2g_to_mg(data_raw_acceleration[0]);
      acceleration_mg[1] = lsm6ds3tr_c_from_fs2g_to_mg(data_raw_acceleration[1]);
      acceleration_mg[2] = lsm6ds3tr_c_from_fs2g_to_mg(data_raw_acceleration[2]);
      usb_echo("Acceleration [mg]:%x\t%x\t%x\r\n", data_raw_acceleration[0], data_raw_acceleration[1],
               data_raw_acceleration[2]);
      usb_echo("Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n", acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
    }

    if (reg.status_reg.gda) {
      /* Read magnetic field data */
      memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));
      lsm6ds3tr_c_angular_rate_raw_get(&lsm6ds3tr_c_ctx, data_raw_angular_rate);
      angular_rate_mdps[0] = lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate[0]);
      angular_rate_mdps[1] = lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate[1]);
      angular_rate_mdps[2] = lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate[2]);
      usb_echo("Angular rate [mdps]:%x\t%x\t%x\r\n", data_raw_angular_rate[0], data_raw_angular_rate[1],
               data_raw_angular_rate[2]);
      //  usb_echo("Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n", angular_rate_mdps[0], angular_rate_mdps[1],
      //           angular_rate_mdps[2]);
    }

    if (reg.status_reg.tda) {
      /* Read temperature data */
      memset(&data_raw_temperature, 0x00, sizeof(int16_t));
      lsm6ds3tr_c_temperature_raw_get(&lsm6ds3tr_c_ctx, &data_raw_temperature);
      temperature_degC = lsm6ds3tr_c_from_lsb_to_celsius(data_raw_temperature);
      // usb_echo("Temperature [degC]:%6.2f\r\n", temperature_degC);
      usb_echo("Temperature [degC]:%x\r\n", data_raw_temperature);
    }
#endif

#if 0
    uint8_t reg;
    /* Read output only if new value is available */
    lis2mdl_mag_data_ready_get(&lis2mdl_ctx, &reg);

    if (reg) {
      /* Read magnetic field data */
      memset(data_raw_magnetic, 0x00, 3 * sizeof(int16_t));
      lis2mdl_magnetic_raw_get(&lis2mdl_ctx, data_raw_magnetic);
      magnetic_mG[0] = lis2mdl_from_lsb_to_mgauss(data_raw_magnetic[0]);
      magnetic_mG[1] = lis2mdl_from_lsb_to_mgauss(data_raw_magnetic[1]);
      magnetic_mG[2] = lis2mdl_from_lsb_to_mgauss(data_raw_magnetic[2]);
      //  usb_echo("Magnetic field [mG]:%4.2f\t%4.2f\t%4.2f\r\n", magnetic_mG[0], magnetic_mG[1], magnetic_mG[2]);
      usb_echo("Magnetic field [mG]:%x\t%x\t%x\r\n", data_raw_magnetic[0], data_raw_magnetic[1], data_raw_magnetic[2]);
      //  sprintf((char *)tx_buffer, "Magnetic field [mG]:%4.2f\t%4.2f\t%4.2f\r\n", magnetic_mG[0], magnetic_mG[1],
      //          magnetic_mG[2]);
      //  tx_com(tx_buffer, strlen((char const *)tx_buffer));
      /* Read temperature data */
      memset(&data_raw_temperature, 0x00, sizeof(int16_t));
      lis2mdl_temperature_raw_get(&lis2mdl_ctx, &data_raw_temperature);
      temperature_degC = lis2mdl_from_lsb_to_celsius(data_raw_temperature);
      // usb_echo("Temperature [degC]:%6.2f\r\n", temperature_degC);
      usb_echo("Temperature [degC]:%x\r\n", data_raw_temperature);
      //  sprintf((char *)tx_buffer, "Temperature [degC]:%6.2f\r\n", temperature_degC);
      //  tx_com(tx_buffer, strlen((char const *)tx_buffer));
    }

#endif
    vTaskDelay(100);
  }
  return;
}
#if defined(__CC_ARM) || (defined(__ARMCC_VERSION)) || defined(__GNUC__)
int main(void)
#else
void main(void)
#endif
{
    BOARD_ConfigMPU();
    BOARD_InitBootPins();
    BOARD_InitBootClocks();
    BOARD_InitDebugConsole();
    BOARD_InitBootPeripherals();

    /* Disable LDOGPADC power down */
    //POWER_DisablePD(kPDRUNCFG_PD_LDOGPADC); // LPADC need

    lwrb_init(&msg, msg_buff, MSG_BUFFER_SIZE * sizeof(msg_buffer));

    if (xTaskCreate(APP_task,                                   /* pointer to the task */
                    "app task",                                 /* task name for kernel awareness debugging */
                    5000L / sizeof(portSTACK_TYPE),             /* task stack size */
                    &g_UsbDeviceComposite,                      /* optional task startup argument */
                    4U,                                         /* initial priority */
                    &g_UsbDeviceComposite.applicationTaskHandle /* optional task handle to create */
                    ) != pdPASS) {
      usb_echo("app task create failed!\r\n");
#if (defined(__CC_ARM) || (defined(__ARMCC_VERSION)) || defined(__GNUC__))
        return 1U;
#else
        return;
#endif
    }

    if (xTaskCreate(Joystick_Task,                  /* pointer to the task */
                    "joystick task",                /* task name for kernel awareness debugging */
                    5000L / sizeof(portSTACK_TYPE), /* task stack size */
                    NULL,                           /* optional task startup argument */
                    5U,                             /* initial priority */
                    &joystickTaskHandle             /* optional task handle to create */
                    ) != pdPASS) {
      usb_echo("joystick task create failed!\r\n");

#if (defined(__CC_ARM) || (defined(__ARMCC_VERSION)) || defined(__GNUC__))
      return 1U;
#else
      return;
#endif
    }

    if (xTaskCreate(Touch_Task,                     /* pointer to the task */
                    "Touch task",                   /* task name for kernel awareness debugging */
                    5000L / sizeof(portSTACK_TYPE), /* task stack size */
                    NULL,                           /* optional task startup argument */
                    3U,                             /* initial priority */
                    &touchTaskHandle                /* optional task handle to create */
                    ) != pdPASS) {
      usb_echo("touch task create failed!\r\n");
#if (defined(__CC_ARM) || (defined(__ARMCC_VERSION)) || defined(__GNUC__))
      return 1U;
#else
      return;
#endif
    }

    if (xTaskCreate(Sensor_Task,                    /* pointer to the task */
                    "Sensor task",                  /* task name for kernel awareness debugging */
                    5000L / sizeof(portSTACK_TYPE), /* task stack size */
                    NULL,                           /* optional task startup argument */
                    3U,                             /* initial priority */
                    &sensorTaskHandle               /* optional task handle to create */
                    ) != pdPASS) {
      usb_echo("sensor task create failed!\r\n");
#if (defined(__CC_ARM) || (defined(__ARMCC_VERSION)) || defined(__GNUC__))
      return 1U;
#else
      return;
#endif
    }

    vTaskStartScheduler();

#if (defined(__CC_ARM) || (defined(__ARMCC_VERSION)) || defined(__GNUC__))
    return 1U;
#endif
}
